#include <stdio.h>
#include "hi_gpio.h"
#include "hi_time.h"
#include "hi_watchdog.h"

#include "servo.h"

#define t_KP (3500)
#define t_KI (0)
#define t_KD (1000)
#define MAX_INT (200)

/***************************************************
 * @brief send pwm to Servo
 * 1.Due to the high frequence of 3861,we have to
 * mock pwm by using normal gpio
 * 2.The servo changes one fangwei every 20us in
 * the period of 10ms, and is 0.01V in val
 * 3.The servo(S3010) is 50Hz, its midangle is
 * around 0.17V
 * test:90.--around 0.3V --duty=11%
 * duty加100，逆时针转10度左右，duty为1500的时候正好在中间（频率为100HZ）
 ****************************************************/
void Servo_Send_PWM(int duty)
{
    int ret = 0;


    ret = hi_gpio_set_ouput_val(HI_GPIO_SERVO, HI_GPIO_VALUE1);
    printf("duty: %d\n", duty);
    if (ret != 0)
    {
        printf("[ERROR] Gpio SetOutputVal high error!");
    }

    hi_udelay(duty);
 
    ret = hi_gpio_set_ouput_val(HI_GPIO_SERVO, HI_GPIO_VALUE0);
    if (ret != 0)
    {
        printf("[ERROR] Gpio SetOutputVal low(0) error!");
    }

    hi_udelay(20000-duty);


}

/***************************************************
 * @brief seek_PD
 * PD control machine
 ****************************************************/
// int Track_PID(float error)
// {
//     static float last_error, int_error;
//     float output;

//     int_error += error;

//     if (int_error > MAX_INT)
//         int_error = MAX_INT;
//     if (int_error < -MAX_INT)
//         int_error = -MAX_INT;

//     output = t_KP * error + t_KI * int_error + t_KD * (error - last_error);

//     last_error = error;

//     return (int)output;
// }

/***************************************************
 * @brief servo control by PID
 * control servo by PID
 ****************************************************/
void *Servo_Control(void *param)
{
    int *duty = (int *)param;
    // real_angle = Track_PID(angle);
    
    while (1)
    {
        Servo_Send_PWM(1500);
        hi_watchdog_feed();
    }
    
}
